#include "SIFTTracker.h"
#include <limits>
unsigned int Tracker::mIDCount = 0;


//Important : When you call these function, Feature of mLastSeenObject and inDATObject 
//could already have been calculated
bool SIFTTracker::compareObjects(DATObject* inDATObject, double* outConfidence)
{
	DATObjectSIFT* inDATObjectSIFT = (DATObjectSIFT*)inDATObject;
	DATObjectSIFT* mLastSeenDATObjectSIFT = (DATObjectSIFT*)mLastSeenDATObject;

	//check something first... don't judge me...
	double lLastWidth, lLastHeight, lLastX, lLastY, lLastFrame;
	if(mLastSeenDATObject->mLastWasDetected)
	{
		lLastWidth = mLastSeenDATObject->mLastPosition.width;
		lLastHeight = mLastSeenDATObject->mLastPosition.height;
		lLastX = mLastSeenDATObject->mLastPosition.x;
		lLastY = mLastSeenDATObject->mLastPosition.y;
		lLastFrame = mLastSeenDATObject->mFramePos;
	}
	else
	{
		for(unsigned int i=mLastSeenDATObject->mHistory.mWasDetected.size()-1;i>=0;i--)
			if(mLastSeenDATObject->mHistory.mWasDetected[i])
			{
				lLastWidth = mLastSeenDATObject->mHistory.mPositions[i].width;
				lLastHeight = mLastSeenDATObject->mHistory.mPositions[i].height;
				lLastX = mLastSeenDATObject->mHistory.mPositions[i].x;
				lLastY = mLastSeenDATObject->mHistory.mPositions[i].y;
				lLastFrame = mLastSeenDATObject->mHistory.mFrameMapping[i];
				break;
			}
	}
	double lSizeRatio = double(inDATObject->mLastPosition.height*inDATObject->mLastPosition.width)/double(lLastWidth*lLastHeight);
	if(lSizeRatio<MIN_SIZE_RATIO || lSizeRatio>MAX_SIZE_RATIO)
	{
		*outConfidence = -1.0;
		return false;	//CHEESE
	}
	int lDelta = inDATObject->mFramePos - lLastFrame;
	double lDistanceRatioX = double(abs(inDATObject->mLastPosition.x-lLastX))/(inDATObject->mLastPosition.width/2);
	double lDistanceRatioY = double(abs(inDATObject->mLastPosition.y-lLastY))/(inDATObject->mLastPosition.height/2);
	if(lDistanceRatioX>MAX_DIST_RATIO*lDelta || lDistanceRatioY > MAX_DIST_RATIO*lDelta)
	{
		*outConfidence = -1.0;
		return false;	//CHEESE
	}

	//worst (best) case, it does nothing :)
	inDATObjectSIFT->calculateSIFTFeatures();
	inDATObjectSIFT->buildKDTree();
	mLastSeenDATObjectSIFT->calculateSIFTFeatures();
	mLastSeenDATObjectSIFT->buildKDTree();

	struct feature* lFeatureTemp;
	struct feature** lNbrs;

	//mLastMatches.clear();

	int m = 0;

	for( int i = 0; i < mLastSeenDATObjectSIFT->mNbOfFeatures ; i++ )
	{
		lFeatureTemp = mLastSeenDATObjectSIFT->mSIFTFeatures + i;
		int k = kdtree_bbf_knn( inDATObjectSIFT->mKDRoot, lFeatureTemp, mNbOfNeighborsToFind, &lNbrs, mMaxNNChks );
		if( k == mNbOfNeighborsToFind )
		{
			double lDist0 = descr_dist_sq( lFeatureTemp, lNbrs[0] );
			double lDist1 = descr_dist_sq( lFeatureTemp, lNbrs[1] );
			if( lDist0 < lDist1 * NN_SQ_DIST_RATIO_THR )
			{
				CvPoint lPt1 = cvPoint( cvRound( lFeatureTemp->x ), cvRound( lFeatureTemp->y ) );
				CvPoint lPt2 = cvPoint( cvRound( lNbrs[0]->x ), cvRound( lNbrs[0]->y ) );

				CvPoint lPt1Real = lPt1;
				lPt1Real.x = lPt1Real.x + lLastX;//mLastSeenDATObject->mLastPosition.x;
				lPt1Real.y = lPt1Real.y + lLastY;//mLastSeenDATObject->mLastPosition.y;
		
				m++;
				mLastSeenDATObjectSIFT->mSIFTFeatures[i].fwd_match = lNbrs[0];
			}
		}
		if(k>0)
			free( lNbrs );
	}
	//David Lowe said that you need at least 3 points
	if(m<3)
	{
		*outConfidence = -1.0;
		return false;
	}
	else
		*outConfidence = (float)m/(float)mLastSeenDATObjectSIFT->mNbOfFeatures;

	return true;
}

bool SIFTTracker::findObjectInFrame(DATFrame* inDATFrame, pair<cv::Point, double>& outRectTransformFound, double* outConfidence)
{

	DATObjectSIFT* mLastSeenDATObjectSIFT = (DATObjectSIFT*)mLastSeenDATObject;
	DATFrameSIFT* inDATFrameSIFT = (DATFrameSIFT*)inDATFrame;

	//worst (best) case, it does nothing :)
	inDATFrameSIFT->calculateSIFTFeatures();
	inDATFrameSIFT->buildKDTree();
	mLastSeenDATObjectSIFT->calculateSIFTFeatures();
	mLastSeenDATObjectSIFT->buildKDTree();

	double lLastWidth, lLastHeight, lLastX, lLastY, lLastFrame;
	if(mLastSeenDATObject->mLastWasDetected)
	{
		lLastWidth = mLastSeenDATObject->mLastPosition.width;
		lLastHeight = mLastSeenDATObject->mLastPosition.height;
		lLastX = mLastSeenDATObject->mLastPosition.x;
		lLastY = mLastSeenDATObject->mLastPosition.y;
		lLastFrame = mLastSeenDATObject->mFramePos;
	}
	else
	{
		for(unsigned int i=mLastSeenDATObject->mHistory.mWasDetected.size()-1;i>=0;i--)
			if(mLastSeenDATObject->mHistory.mWasDetected[i])
			{
				lLastWidth = mLastSeenDATObject->mHistory.mPositions[i].width;
				lLastHeight = mLastSeenDATObject->mHistory.mPositions[i].height;
				lLastX = mLastSeenDATObject->mHistory.mPositions[i].x;
				lLastY = mLastSeenDATObject->mHistory.mPositions[i].y;
				lLastFrame = mLastSeenDATObject->mHistory.mFrameMapping[i];
				break;
			}
	}

	struct feature* lFeatureTemp;
	struct feature** lNbrs;

	mLastMatches.clear();

	int m = 0;
	int n = 0;
	vector<CvPoint2D32f> lPts1,lPts2;

	//the object AND the frame have their features calculated
	for( int i = 0; i < mLastSeenDATObjectSIFT->mNbOfFeatures ; i++ )
	{
		lFeatureTemp = mLastSeenDATObjectSIFT->mSIFTFeatures + i;
		int k = kdtree_bbf_knn( inDATFrameSIFT->mKDRoot, lFeatureTemp, mNbOfNeighborsToFind, &lNbrs, mMaxNNChks );
		if( k == mNbOfNeighborsToFind )
		{
			double lDist0 = descr_dist_sq( lFeatureTemp, lNbrs[0] );
			double lDist1 = descr_dist_sq( lFeatureTemp, lNbrs[1] );
			if( lDist0 < lDist1 * NN_SQ_DIST_RATIO_THR )
			{
				CvPoint lPt1 = cvPoint( cvRound( lFeatureTemp->x ), cvRound( lFeatureTemp->y ) );
				CvPoint lPt2 = cvPoint( cvRound( lNbrs[0]->x ), cvRound( lNbrs[0]->y ) );
				
				CvPoint lPt1Real = lPt1;
				CvPoint lPt2Real = lPt2;
				lPt1Real.x = lPt1Real.x + lLastX;	//The SIFT are calculated inside the DATObject...
				lPt1Real.y = lPt1Real.y + lLastY;	//We add the position of the object to know where de SIFT is.
				lPt2Real.x = lPt2Real.x + inDATFrameSIFT->mROI.x;
				lPt2Real.y = lPt2Real.y + inDATFrameSIFT->mROI.y;

				lPts1.push_back(cvPoint2D32f(lPt1Real.x,lPt1Real.y));
				lPts2.push_back(cvPoint2D32f(lPt2Real.x,lPt2Real.y));

				mLastMatches.push_back(std::pair<CvPoint,CvPoint>(lPt1Real,lPt2Real));

				m++;
				mLastSeenDATObjectSIFT->mSIFTFeatures[i].fwd_match = lNbrs[0];
			}
		}
		if(k>0)
			free( lNbrs );
	}

	n = lPts1.size();

	*outConfidence = (float)m/(float)mLastSeenDATObjectSIFT->mNbOfFeatures;


	CvMat* lMatH = cvCreateMat(3,3,CV_32F);
	float* h = lMatH->data.fl;
	CvMat lVecPt1, lVecPt2;

	CvMat* lMask;

	if( n < 4 )
	{
		*outConfidence = -1.0;
		return false;	//cannot give a good transformation
	}
	lVecPt1 = cvMat(1, n, CV_32FC2, &lPts1[0] );
	lVecPt2 = cvMat(1, n, CV_32FC2, &lPts2[0] );

	lMask = cvCreateMat(1,n,CV_8U);
	bool lOk = cvFindHomography(&lVecPt1, &lVecPt2, lMatH, CV_RANSAC, 1,lMask );
	int lSum = 0;
	for(int i=0;i<n;i++)
		lSum += lMask->data.ptr[i];
		
	if(lSum < 4 || h[8] != h[8] || fabs(h[8]) == std::numeric_limits<double>::infinity())
	{	
		*outConfidence = -1.0;
		cvReleaseMat(&lMatH);
		return false;	//cannot give a good transformation
	}
	CvPoint lCenterSrc = cvPoint(lLastX+lLastWidth/2,lLastY+lLastHeight/2);

	double x = lCenterSrc.x, y = lCenterSrc.y;
	double Z = 1./(h[6]*x + h[7]*y + h[8]);
	double X = abs((h[0]*x + h[1]*y + h[2])*Z);
	double Y = abs((h[3]*x + h[4]*y + h[5])*Z);
	CvPoint lCenterDest = cvPoint(cvRound(X), cvRound(Y));

	//this is where I fuck things up

	outRectTransformFound.first.x = lCenterDest.x-lCenterSrc.x;
	outRectTransformFound.first.y = lCenterDest.y-lCenterSrc.y;

	float lXcSrc = lCenterSrc.x;
	float lYcSrc = lCenterSrc.y;
	float lXcDest = lCenterDest.x;
	float lYcDest = lCenterDest.y;

	double lDistRatio = 0.0;
	double lSizeRatio = 0.0;
	int lCount = 0;
	int lCount2 = 0;
	for(int i=0;i<n;i++)
	{
		CvScalar lGoodPt = cvGet1D(lMask,i);
		if(!lGoodPt.val[0])
			continue;
		x = lPts1[i].x, y = lPts1[i].y;
		Z = 1./(h[6]*x + h[7]*y + h[8]);
		X = (h[0]*x + h[1]*y + h[2])*Z;
		Y = (h[3]*x + h[4]*y + h[5])*Z;
		CvPoint2D32f lPt2Temp = cvPoint2D32f(X, Y);

		float lDistSrc = sqrt(pow(lXcSrc-lPts1[i].x,2)+pow(lYcSrc-lPts1[i].y,2));
		float lDistDest = sqrt(pow(lXcDest-lPts2[i].x,2)+pow(lYcDest-lPts2[i].y,2));
		float lDistDest2 = sqrt(pow(lXcDest-lPt2Temp.x,2)+pow(lYcDest-lPt2Temp.y,2));
		if(!(lDistSrc < MIN_DIST_CENTER || lDistDest < MIN_DIST_CENTER))
		{
			lSizeRatio = lSizeRatio + lDistDest/lDistSrc;
			lCount++;
		}
		if(!(lDistSrc < MIN_DIST_CENTER || lDistDest2 < MIN_DIST_CENTER))
		{
			lCount2++;
		}
	}
	cvReleaseMat(&lMatH);
	lSizeRatio = lSizeRatio/(double)lCount;	//means of ratios
	outRectTransformFound.second = lSizeRatio;

	//apply the transformation. If it's to far, don't accept it !
	double lDistX = abs(outRectTransformFound.first.x)/(lLastWidth*lSizeRatio);
	double lDistY = abs(outRectTransformFound.first.y)/(lLastHeight*lSizeRatio);

	
	int lDelta = inDATFrame->mFramePos - lLastFrame;
	if(lCount<3)
	{
		*outConfidence = -1.0;
		return false;	//CHEESE
	}
	if((lDistX>MAX_DIST_RATIO*lDelta || lDistY>MAX_DIST_RATIO*lDelta)&&lDelta>0)
	{
		*outConfidence = -1.0;
		return false;	//CHEESE
	}
	if(lSizeRatio<MIN_SIZE_RATIO || lSizeRatio>MAX_SIZE_RATIO)
	{
		*outConfidence = -1.0;
		return false;	//CHEESE
	}
	return true;
}
void SIFTTracker::update(DATObject* inDATObject, double inAliveConfidenceThreshold, int inLifeCounterThreshold)
{
	int lDelta = inDATObject->mFramePos - mLastSeenDATObject->mFramePos;

	inDATObject->mHistory = mLastSeenDATObject->mHistory;
	inDATObject->mHistory.mPositions.push_back(mLastSeenDATObject->mLastPosition);
	inDATObject->mHistory.mObjectType.push_back(mLastSeenDATObject->mLastObjectType);	
	inDATObject->mHistory.mWasDetected.push_back(mLastSeenDATObject->mLastWasDetected);
	inDATObject->mHistory.mFrameMapping.push_back(mLastSeenDATObject->mFramePos);
	inDATObject->mHistory.mConfidences.push_back(mLastSeenDATObject->mLastConfidence);
	inDATObject->mLifeCounter = mLastSeenDATObject->mLifeCounter;
	//inDATObject->mDeathCounter = mLastSeenDATObject->mDeathCounter;	//not sure...

	//It will take care of the deallocation
	setLastSeenDATObject(inDATObject);

	double lTmpConf = 1.0;
	for(unsigned int i=0;i<mLastSeenDATObject->mHistory.mConfidences.size();i++)
		lTmpConf *= (1-mLastSeenDATObject->mHistory.mConfidences[i]);
	lTmpConf *= 1-mLastSeenDATObject->mLastConfidence;
	lTmpConf = 1-lTmpConf;
	mLastSeenDATObject->mGeneralConfidence = lTmpConf;
	mLastSeenDATObject->mLifeCounter++;
	if(mLastSeenDATObject->mLifeCounter>inLifeCounterThreshold && mLastSeenDATObject->mGeneralConfidence>inAliveConfidenceThreshold)
	{
		mLastSeenDATObject->mLivingState = Living;
	}
	mLastWasTracked = 1;

	//correctKalman
	//The return value is not used
	//TODO::Prediction should be inside the compare function ?
	cv::Rect lTmp = predictionKalman(lDelta);
	cv::Rect lTmp2 = correctKalman(mLastSeenDATObject->mLastPosition);
	cv::Rect lTmp3 = mLastSeenDATObject->mHistory.mPositions[mLastSeenDATObject->mHistory.mPositions.size()-1];
	//cout<<"ID: "<<mID<<endl;
	//cout<<"Transition:"<<endl;
	//cout<<mKF.transitionMatrix<<endl;
	//cout<<"Last position:"<<lTmp3.x<<" "<<lTmp3.y<<" "<<lTmp3.width<<" "<<lTmp3.height<<" "<<endl;
	//cout<<"Prediction:"<<lTmp.x<<" "<<lTmp.y<<" "<<lTmp.width<<" "<<lTmp.height<<" "<<endl;
	//cout<<"Measure:"<<mLastSeenDATObject->mLastPosition.x<<" "<<mLastSeenDATObject->mLastPosition.y<<" "<<mLastSeenDATObject->mLastPosition.width<<" "<<mLastSeenDATObject->mLastPosition.height<<" "<<endl;
	//cout<<"Estimated:"<<lTmp2.x<<" "<<lTmp2.y<<" "<<lTmp2.width<<" "<<lTmp2.height<<" "<<endl;
}

void SIFTTracker::update(std::pair<cv::Point,double>& inRectTransformFound, cv::Mat& inFrame, unsigned int inFramePos)
{
	cv::Rect lTmpPosition;
	//0.5 because we want the object to enlarge in every direction (no just to the right)
	//x2 = x1 + tx - (w1*scale-w1)/2
	lTmpPosition.x = mLastSeenDATObject->mLastPosition.x + inRectTransformFound.first.x - (mLastSeenDATObject->mLastPosition.width*inRectTransformFound.second-mLastSeenDATObject->mLastPosition.width)*0.5;
	lTmpPosition.y = mLastSeenDATObject->mLastPosition.y + inRectTransformFound.first.y - (mLastSeenDATObject->mLastPosition.height*inRectTransformFound.second-mLastSeenDATObject->mLastPosition.height)*0.5;
	lTmpPosition.height = mLastSeenDATObject->mLastPosition.height*inRectTransformFound.second;
	lTmpPosition.width = mLastSeenDATObject->mLastPosition.width*inRectTransformFound.second;
	int lDelta = inFramePos - mLastSeenDATObject->mFramePos;
	if(lDelta <= 0)
	{
		std::cerr<<"Matching update: Problem with the delta while updating."<<std::endl;
		lDelta = 1;
	}
	//Kalman filter
	cv::Rect lEstimatedPosition = correctKalman(lTmpPosition);

	mLastSeenDATObject->mHistory.mPositions.push_back(mLastSeenDATObject->mLastPosition);
	mLastSeenDATObject->mHistory.mObjectType.push_back(mLastSeenDATObject->mLastObjectType);
	mLastSeenDATObject->mHistory.mWasDetected.push_back(mLastSeenDATObject->mLastWasDetected);
	mLastSeenDATObject->mHistory.mFrameMapping.push_back(mLastSeenDATObject->mFramePos);
	mLastSeenDATObject->mHistory.mConfidences.push_back(mLastSeenDATObject->mLastConfidence);

	mLastSeenDATObject->mLastPosition = lEstimatedPosition;
	mLastSeenDATObject->mLastWasDetected = false;
	mLastSeenDATObject->mLastObjectType=""; //Cannot be sure
	mLastSeenDATObject->mFramePos = inFramePos;
	//NEW NEW NEW 
	mLastSeenDATObject->mDeathCounter = mLastSeenDATObject->mDeathCounter - 0.5;
	if(mLastSeenDATObject->mDeathCounter<=0)
	{
		if(getLivingState() == Living)
		{ //Its really dead :(
			setLivingState(Dead);
		}
		else
		{ //Never was alive !
			setLivingState(Undead);
		}	
	}

	mLastWasTracked = 1;
}
//==== DATObjectSIFT function ====//
bool DATObjectSIFT::calculateSIFTFeatures()
{
	int lFramePos = mFramePos;
	if(!mLastWasDetected)
		for(unsigned int i=mHistory.mWasDetected.size()-1;i>=0;i--)
			if(mHistory.mWasDetected[i])
			{
				lFramePos = mHistory.mFrameMapping[i];
				break;
			}
	if(lFramePos!=mSIFTFeaturesFramePos)
	{
		clearSIFT();
		mSIFTFeaturesFramePos = mFramePos;
	}
	if(!mLastSeenObject.empty() && !mSIFTFeatures)
	{
		IplImage lTmp = mLastSeenObject;
		mNbOfFeatures = _sift_features(&lTmp,&mSIFTFeatures,mIntvls,mSigma,mContrThr,mCurvThr,mImgDbl,mDescrWidth,mDescrHistBins);
		return true;
	}
	return false;
}

bool DATObjectSIFT::buildKDTree()
{
	if(!mLastSeenObject.empty() && mSIFTFeatures && !mKDRoot)
	{
		mKDRoot = kdtree_build( mSIFTFeatures, mNbOfFeatures );
		return true;
	}
	return false;
}

void DATObjectSIFT::clearSIFT()
{
	if(mSIFTFeatures)
	{
		free(mSIFTFeatures);
		mSIFTFeatures = NULL;
	}
	if(mKDRoot)
	{
		kdtree_release(mKDRoot);
		mKDRoot = NULL;
	}
}

bool DATFrameSIFT::calculateSIFTFeatures()
{
	if(mROI.x != mLastROI.x && mROI.y != mLastROI.y && mROI.width != mLastROI.width && mROI.height != mLastROI.height)
	{
		clearSIFT();
		mLastROI = mROI;
	}
	if(!mActualFrame.empty() && !mSIFTFeatures)
	{
		IplImage lTmp = mActualFrame;
		mNbOfFeatures = _sift_features(&lTmp,&mSIFTFeatures,mIntvls,mSigma,mContrThr,mCurvThr,mImgDbl,mDescrWidth,mDescrHistBins);
		return true;
	}
	return false;
}

bool DATFrameSIFT::buildKDTree()
{
	if(!mActualFrame.empty() && mSIFTFeatures && !mKDRoot)
	{
		mKDRoot = kdtree_build( mSIFTFeatures, mNbOfFeatures );
		return true;
	}
	return false;
}

void DATFrameSIFT::clearSIFT()
{
	if(mSIFTFeatures)
	{
		free(mSIFTFeatures);
		mSIFTFeatures = NULL;
	}
	if(mKDRoot)
	{
		kdtree_release(mKDRoot);
		mKDRoot = NULL;
	}
}
